#include <iostream>
#include <fstream>
#include <stdio.h>
#define _USE_MATH_DEFINES
#include <math.h>

#include "graph.h"
#include "matrix.h"
#include "vector.h"

using namespace std;

inline double sq(double d) {return d*d;}

class Point 
{
public:
	double x,y,z;
	Point() : x(0), y(0), z(0) {}
	Point(double _x, double _y, double _z) : x(_x), y(_y), z(_z) {}
	Point& operator+=(const Point& p);
};

Point& Point::operator+=(const Point& p)
{
	x += p.x;
	y += p.y;
	z += p.z;
	return *this;
}


// an edge will connect vertices a to b. It faces triangles tri1 and tri2
struct Edge
{
	unsigned int a, b;
	int tri1, tri2; // -1 means it has not been initialised
};

Point operator-(const Point& A, const Point& B)
{
	Point C;
	C.x=A.x-B.x;
	C.y=A.y-B.y;
	C.z=A.z-B.z;
	return C;
}
Point operator+(const Point& A, const Point& B)
{
	Point C;
	C.x=A.x+B.x;
	C.y=A.y+B.y;
	C.z=A.z+B.z;
	return C;
}

Point operator*(double d, const Point& P)
{
	Point C;
	C.x = d*P.x;
	C.y = d*P.y;
	C.z = d*P.z;
	return C;
}

// cross product
Point operator*(const Point& A, const Point& B)
{
	Point C;
	C.x=A.y*B.z-A.z*B.y;
	C.y=A.x*B.z-A.z*B.x;
	C.z=A.x*B.y-A.y*B.x;
	return C;
}

double module(const Point& p)
{
	return sqrt(p.x*p.x+p.y*p.y+p.z*p.z);
}


double surface(Point& p, Point& q, Point& r)
{
	return 0.5*module((p-q)*(p-r));
}

Point normal(Point& p, Point& q, Point& r)
{
	return (p-q)*(p-r);
}


class mesh3d
{
public:
	mesh3d() : nbTriangles(0), maxEdges(0), nbPoints(0), edges(NULL), vertices(NULL), hasTrianglesFromEdges(false) {}
	~mesh3d() { free();}

	void init(int _nbTriangles, int _nbPoints);
	void free();

	void addVertex(const Point& p);
	int addEdge(const int a, const int b); // Undirected edge goes from Vertex[a] to Vertex [b].
	int addEdge2(const int a, const int b); // Undirected edge goes from Vertex[a] to Vertex [b].
	void addTriangleByVertices(const int a, const int b, const int c); // Triangle determined by Vertex[a], Vertex [b], Vertex [c].
	void addTriangleByEdges(const unsigned int a, const unsigned int b, const unsigned int c); // Triangle determined by Vertex[a], Vertex [b], Vertex [c].
	void addTriangleToEdge(const int edge, const unsigned int tri);

	int getNbEdges() {return g.getNbEdges();}
	int valence(unsigned int vertex) {return g.valence(vertex);}

	void buildTrianglesFromEdges();
	void setMaxEdges (int _maxEdges) { maxEdges = _maxEdges; edges = new Edge[maxEdges]; for(int i=0; i<maxEdges; ++i) {edges[i].tri1=-1; edges[i].tri2=-1;}}


	// generates
	mesh3d* loop_subdivision();

	Point* vertices;
	Edge * edges;
	HuMatrix<int> triangles, trianglesFromEdges;
	int nbTriangles, nbPoints;

protected:
	Graph<unsigned int> g; // connectivity info about points and vertices
	bool hasTrianglesFromEdges;
	int maxEdges;
	int nbTrianglesDefined;

// internally used by loop_subdivision();
	
	// shifts the edge as per loop subdivision [Hamid-Reza Pakdel, Faramarz F. Samavati - Incremental Subdivision for triangle meshes], formula (4).
	Point shiftEdge(unsigned int e);
	// returns the third point of the trianlge, e.d. not part of the edge.
	unsigned int thirdPoint(unsigned int e, unsigned int triangle);

};


	// returns the third point of the trianlge, e.d. not part of the edge.
unsigned int mesh3d::thirdPoint(unsigned int e, unsigned int triangle)
{
	int aux = triangles(triangle,0);
	aux += triangles(triangle,1);
	aux += triangles(triangle,2);
	aux -= edges[e].a + edges[e].b;

	return triangles(triangle,0)+triangles(triangle,1)+triangles(triangle,2) - edges[e].a - edges[e].b;
}


int mesh3d::addEdge(const int a, const int b) // Undirected edge goes from Vertex[a] to Vertex [b], both a and b are part of triangles[tri].
{
	int edge = g.insertEdge(a, b);
	edges[edge].a=a;
	edges[edge].b=b;
	//addTriangleToEdge(nr, tri);
	return edge;
}


int mesh3d::addEdge2(const int a, const int b) // Undirected edge goes from Vertex[a] to Vertex [b], both a and b are part of triangles[tri].
{
	int edge=55;
	edge = g.insertEdge(a, b);
	edges[edge].a=a;
	edges[edge].b=b;
	//addTriangleToEdge(nr, tri);
	return edge;
}

void mesh3d::addTriangleToEdge(const int edge, const unsigned int tri)
{
	if (edges[edge].tri1==-1)
	{
		edges[edge].tri1 = tri;
	} else
	{
		edges[edge].tri2 = tri;
	}
}



void mesh3d::init(int _nbTriangles, int _nbPoints)
{
	nbTriangles = _nbTriangles;
	nbPoints = _nbPoints;
	triangles = HuMatrix<int>(nbTriangles,3);
	vertices = new Point[nbPoints];
	nbTrianglesDefined = 0;
	g.init(nbPoints);
}

void mesh3d::free()
{
	if (vertices) delete[] vertices;
	if (edges) delete[] edges;
}

void mesh3d::buildTrianglesFromEdges()
{
	unsigned int a, b, c;
	int i;
	if (hasTrianglesFromEdges) return;

	trianglesFromEdges = HuMatrix<int>(nbTriangles, 3);

	for (i=0; i<nbTriangles; ++i)
	{
		a = addEdge(triangles(i,0), triangles(i,1));
		b = addEdge(triangles(i,1), triangles(i,2));
		c = addEdge(triangles(i,2), triangles(i,0));
		//aux = thirdPoint(edges[a], i);

		addTriangleByEdges(a,b,c);

	} // for (i=0; i<CTRIA3; ++i)
	hasTrianglesFromEdges = true;
	g.completeVertexInfo();
} // void mesh::buildTrianglesFromEdges()


void mesh3d::addTriangleByEdges(const unsigned int a, const unsigned int b, const unsigned int c)
{
	trianglesFromEdges(nbTrianglesDefined, 0) = a;
	trianglesFromEdges(nbTrianglesDefined, 1) = b;
	trianglesFromEdges(nbTrianglesDefined, 2) = c;

	addTriangleToEdge(a,nbTrianglesDefined);
	addTriangleToEdge(b,nbTrianglesDefined);
	addTriangleToEdge(c,nbTrianglesDefined);

	nbTrianglesDefined++;
}

void save_gts(char* filename, mesh3d* mesh)
{
	int i;
	ofstream out(filename);

	mesh->buildTrianglesFromEdges();

	out<<mesh->nbPoints<<" "<<mesh->getNbEdges()<<" "<<mesh->nbTriangles<<endl;;
	for (i=0; i<mesh->nbPoints; ++i)
	{
		out<<mesh->vertices[i].x<<" "<<mesh->vertices[i].y<<" "<<mesh->vertices[i].z<<endl;
	}

	for (i=0; i<mesh->getNbEdges(); ++i)
	{
		out<<mesh->edges[i].a+1<<" "<<mesh->edges[i].b+1<<endl;
	}

	for (i=0; i<mesh->nbTriangles; ++i)
	{
		out<<mesh->trianglesFromEdges(i,0)+1<<" "<<mesh->trianglesFromEdges(i,1)+1<<" "<<mesh->trianglesFromEdges(i,2)+1<<endl;
	}
	out.close();
} // void save_gts(char* filename)


mesh3d* load_nas(char* filename, int offset)
{
	int GRIDStar; // grid coordinates in long format
	int GRID;	// grid coordinates
	int CTRIA3;	// 3-node tria surface faces
	int CQUAD4;	// 4-node quad surface faces
	int CHEXA;	// 8-node hexahedral elements
	int CPENTA5;	// 5-node pentahedral elements (node 5 equal to node 6)
	int CPENTA6;	// 6-node pentahedral elements
	int CTETRA;	// 4-node tetrahedral elements

	cout<<"Loading "<<filename<<endl;
	ifstream in(filename);
	if (in.bad()) return NULL;

	const int LEN=128;
	char line[LEN];
	int i;

	mesh3d* mesh = new mesh3d();

	GRIDStar=0; // grid coordinates in long format
	GRID=0;	// grid coordinates
	CTRIA3=0;	// 3-node tria surface faces
	CQUAD4=0;	// 4-node quad surface faces
	CHEXA=0;	// 8-node hexahedral elements
	CPENTA5=0;	// 5-node pentahedral elements (node 5 equal to node 6)
	CPENTA6=0;	// 6-node pentahedral elements
	CTETRA=0;	// 4-node tetrahedral elements
	while (!in.eof())
	{
		in.getline(line, LEN);
		if (strstr(line,"GRID*")!=NULL) GRIDStar++;
		if (strstr(line,"GRID")!=NULL) GRID++;
		if (strstr(line,"CTRIA3")!=NULL) CTRIA3++;
		if (strstr(line,"CQUAD4")!=NULL) CQUAD4++;
		if (strstr(line,"CHEXA")!=NULL) CHEXA++;
		if (strstr(line,"CPENTA")!=NULL) {CPENTA5++; throw "Not supported";}
		if (strstr(line,"CTETRA")!=NULL) CTETRA++;
	}
	cout<<"GRID="<<GRID<<endl;
	cout<<"CTRIA3="<<CTRIA3<<endl;
	in.clear(0); // need to clear eof flag, since seekg fails if end of the file has bean already reached
	in.seekg(0); // move to 1st position

	mesh->init(CTRIA3,GRID);

	int n, a, b, c;
	double x, y, z;
	for (i=0; i<GRID; ++i)
	{
		//in.getline(line, LEN);
		in>>line;
		in>>n;
		in>>x>>y>>z;
		mesh->vertices[i] = Point(x,y,z);
	}
	for (i=0; i<CTRIA3; ++i)
	{
		in>>line;
		in>>n>>n;
		in>>a>>b>>c;
		if (i==2629)
			i=2629;

		mesh->triangles(i,0)=a-offset;
		mesh->triangles(i,1)=b-offset;
		mesh->triangles(i,2)=c-offset;
	}
	return mesh;
} // mesh3d* nastran::load_nas(char* filename)

Point mesh3d::shiftEdge(unsigned int e)
{
//	cout<<e<<endl;
	Point p;
	p=0.375*(vertices[edges[e].a]+vertices[edges[e].b]);
	p+=0.125*(vertices[thirdPoint(e, edges[e].tri1)] + vertices[thirdPoint(e, edges[e].tri2)]);
	return p;
}

std::ostream& operator<<(std::ostream& out, const Point& p)
{
	out<<p.x<<" "<<p.y<<" "<<p.z;
	return out;
}

mesh3d* mesh3d::loop_subdivision()
// return a new mesh object resulting from applying a loop subdivision to the current mesh.
// the current mesh is unaffected
{
	if (!hasTrianglesFromEdges) buildTrianglesFromEdges();

	mesh3d* sub = new mesh3d();

	sub->init(nbTriangles*4, nbPoints+nbTriangles*3);
	sub->setMaxEdges(maxEdges*9);
	int i, j, n, n2;
	double alpha, beta;
	Point p;
	// Reposition existing Verices
	for (i=0; i<nbPoints; ++i)
	{
		if (i==1156)
			i=1156;

		n = valence(i);
		// ignore points that are not part of a triangle
		if (n<1) continue;
		alpha = (0.625-sq(0.375+0.25*cos(2*M_PI/n)));
		beta = 1-n * alpha;
		p = beta * vertices[i];
		for (j=0; j<n; ++j)
		{			
			n2 = g.neighbour(i,j);
			p+= alpha*vertices[n2];
		}
		sub->vertices[i] = p;
	}
	// Reposition Verices create on the midPoint of the edges
	for (i=0; i<nbTriangles; ++i) //i loops over all existing triangles
	{

		sub->vertices[nbPoints+3*i] = shiftEdge(trianglesFromEdges(i,0));
		sub->vertices[nbPoints+3*i+1] = shiftEdge(trianglesFromEdges(i,1));
		sub->vertices[nbPoints+3*i+2] = shiftEdge(trianglesFromEdges(i,2));
	}

	/// recreate triangles
	int a,b,c,d,e;
	sub->trianglesFromEdges = HuMatrix<int>(nbTriangles*4, 3);;

	for (i=0; i<nbTriangles; ++i) //i loops over all existing triangles
	{
		a = sub->addEdge2(triangles(i,0), nbPoints+3*i);
		b = sub->addEdge2(nbPoints+3*i, nbPoints+3*i+2); // new edge for inner triangle
		c = sub->addEdge2(nbPoints+3*i+2, triangles(i,0));
		sub->addTriangleByEdges(a,b,c);

		d = sub->addEdge2(nbPoints+3*i, nbPoints+3*i+1); // new edge for inner triangle
		e = sub->addEdge2(nbPoints+3*i+1, nbPoints+3*i+2); // new edge for inner triangle
		sub->addTriangleByEdges(b,d,e);


		a = sub->addEdge2(nbPoints+3*i,triangles(i,1));
		c = sub->addEdge2(triangles(i,1), nbPoints+3*i+1); // new edge for inner triangle
		sub->addTriangleByEdges(a,c,d);

		a = sub->addEdge2(nbPoints+3*i+1,triangles(i,2));
		c = sub->addEdge2(triangles(i,2), nbPoints+3*i+2); // new edge for inner triangle
		sub->addTriangleByEdges(a,c,e);
	}
	sub->hasTrianglesFromEdges = true;
	return sub;
} // mesh3d* mesh3d::loop_subdivision()

int magic (int m, int* h)
{
	int i, j;
	j=0;
	for (i=0; i<17; ++i)
	{
		if (m>h[i]) j++;
	}
	return m-j;
}

int main()
{
//	mesh3d* mesh = load_nas("elephant.nas", 0);
	mesh3d* mesh = load_nas("person1_arms_up.nas", 1);
	ofstream out;
	out.open("out.txt");

	int hibak[17] = {1156,1158,1159,1160,1161,1162,1300,1301,2880,2883,2893,2895,2896,2897,2898,2899,3025};
	int i,j;
	bool aux;
/*	for (i=0; i<mesh->nbPoints; ++i)
	{
		aux =  false;
		for (j=0; j<mesh->nbTriangles; ++j)
		{
			if (mesh->triangles(j,0)==i)
			{
				aux=true;
				break;
			}
			if (mesh->triangles(j,1)==i)
			{
				aux=true;
				break;
			}
			if (mesh->triangles(j,2)==i)
			{
				aux=true;
				break;
			}
		}
		if (!aux)
			cout<<i<<endl;	
	}
*/
	out<<"OFF"<<endl;
	out<<mesh->nbPoints-17<<" "<<mesh->nbTriangles<<" 0"<<endl;
	for (i=0; i<mesh->nbPoints; ++i)
	{
		aux =  false;
		for (j=0; j<17; ++j)
		{
			if (i==hibak[j])
			{
				aux =true; break;
			}
		}
		if (!aux)
		{
			out<<mesh->vertices[i].x<<" "<<mesh->vertices[i].y<<" "<<mesh->vertices[i].z<<endl;
		}
	}

	for (i=0; i<mesh->nbTriangles; ++i)
	{
		out<<3<<" "<<magic(mesh->triangles(i,0), hibak)<<" "<<magic(mesh->triangles(i,1),hibak)<<" "<<magic(mesh->triangles(i,2),hibak)<<endl;
	}

	out.close();

	mesh->setMaxEdges(10200);
	save_gts("baba.gts", mesh);
//	save_gts("elephant.gts", mesh);

	if (!mesh)
		cout<<"Nas file loaded succesfully."<<endl;

	mesh3d* mesh2 = mesh->loop_subdivision();
//	save_gts("baba2.gts", mesh2);
	mesh3d* mesh3 = mesh2->loop_subdivision();
	save_gts("baba3.gts", mesh3);
	cout<<"Done. Pres ENTER to continue."<<endl;
	cin.get();
}